# Libraries
import RPi.GPIO as GPIO
import time

# GPIO Mode (BOARD / BCM)
GPIO.setmode(GPIO.BCM)

# set GPIO Pins
GPIO_TRIGGER = 2
GPIO_ECHO = 3
GPIO_LED = 4

# set LED init state
state = False

# set GPIO direction (IN / OUT)
GPIO.setup(GPIO_TRIGGER, GPIO.OUT)
GPIO.setup(GPIO_ECHO, GPIO.IN)
GPIO.setup(GPIO_LED, state)

# total distance
height = 16.5


def distance():
    # set Trigger to HIGH
    GPIO.output(GPIO_TRIGGER, True)

    # set Trigger after 0.01ms to LOW
    time.sleep(0.00001)
    GPIO.output(GPIO_TRIGGER, False)

    StartTime = time.time()
    StopTime = time.time()

    # save StartTime
    while GPIO.input(GPIO_ECHO) == 0:
        StartTime = time.time()

    # save time of arrival
    while GPIO.input(GPIO_ECHO) == 1:
        StopTime = time.time()

    # time difference between start and arrival
    TimeElapsed = StopTime - StartTime
    # multiply with the sonic speed (34300 cm/s)
    # and divide by 2, because there and back
    distance = (TimeElapsed * 34300) / 2

    return distance


def bling():
    pwm = GPIO.PWM(GPIO_LED, 60)
    pwm.start(0)
    for i in range(3):
        for i in range(1, 101, 2):
            pwm.ChangeDutyCycle(i)
            time.sleep(0.01)
        for i in range(100, -1, -2):
            pwm.ChangeDutyCycle(i)
            time.sleep(0.01)
    pwm.stop()


if __name__ == '__main__':
    try:
        while True:
            dist = height - distance()
            print("Measured Distance = %.1f cm" % dist)
            if dist > 12:
                print("Warning: distance beyond 12cm !")
                bling()
            else:
                GPIO.output(GPIO_LED, GPIO.LOW)
            time.sleep(1)

    # Reset by pressing CTRL + C
    except KeyboardInterrupt:
        print("Measurement stopped by User")
        GPIO.cleanup()
